#include "MainTask.h"
#include "MecanumChassis.h"
#include "BSP.h"
#include "JY931.h"

osThreadId_t MainTaskHandle;
const osThreadAttr_t MainTask_attributes = {
    .name = "MainTask",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityLow1,
};
/* 主任务 */
void StartMainTask(void *argument)
{
    while (1)
    {
        /* 设置底盘       x  y  z   方向的速度 */
        Chassis_SetSpeed(0, 0.2, 0.3);
        osDelay(5);
    }
}

osThreadId_t SerialTaskHandle;
const osThreadAttr_t SerialTask_attributes = {
    .name = "SerialTask",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityLow,
};
/* 串口任务 */
void StartSerialTask(void *argument)
{
    while (1)
    {
        // UART_Print("yaw:%d\n", (int)JY931.yaw);
        UART_Print("%d %d %d %d\n", chassis.Motors_Speed[0], chassis.Motors_Speed[1], chassis.Motors_Speed[2], chassis.Motors_Speed[3]);
        osDelay(50);
    }
}

//-----------------------------------------------------------
// 任务初始化
void TaskInit(void)
{
    Chassis_Init();

    MainTaskHandle = osThreadNew(StartMainTask, NULL, &MainTask_attributes);
    SerialTaskHandle = osThreadNew(StartSerialTask, NULL, &SerialTask_attributes);
}
